#include "fpal.hpp" // 包含 FPAL 库头文件
#include <iostream>
#include <iomanip>
#include <cmath>    // 用于浮点数参考值计算

// 定义一个方便的别名，因为所有库内容都在fpal命名空间
namespace fp = fpal;

// 辅助函数，用于检查 ap_fixed 是否接近另一个值
template<typename T>
bool is_close(T a, T b, double epsilon = 1e-6) { 
    // 对于 ap_fixed 类型，直接比较值可能因为精度而失败
    // 转换为 double 进行比较更可靠，但要注意 ap_fixed 的实际精度限制
    return std::abs(static_cast<double>(a) - static_cast<double>(b)) < epsilon;
}

// 测试平台主函数
int main() {
    std::cout << "--- FPAL HLS Testbench ---" << std::endl;
    // 设置浮点数输出精度，高精度有助于观察ap_fixed的实际值
    std::cout << std::fixed << std::setprecision(10); 

    // --- 手动计算并定义 ap_fixed 类型的精确最大/最小值 ---
    // fp_coord_t (W=32, I=18, F=14): Max = 2^17 - 2^-14, Min = -2^17
    fp::fp_coord_t max_coord_val = (fp::fp_coord_t)(131072.0 - std::pow(2, -14)); // 131071.99993896484375
    fp::fp_coord_t min_coord_val = (fp::fp_coord_t)(-131072.0);

    // fp_vec_unit_t (W=24, I=3, F=21): Max = 2^2 - 2^-21, Min = -2^2
    fp::fp_vec_unit_t max_vec_unit_val = (fp::fp_vec_unit_t)(4.0 - std::pow(2, -21)); // 3.999999523162841796875
    fp::fp_vec_unit_t min_vec_unit_val = (fp::fp_vec_unit_t)(-4.0);


    // 添加调试代码：打印 ap_fixed 类型的精确最大/最小值
    std::cout << "\n--- Debugging numeric_limits max/min values ---" << std::endl;
    // 使用我们手动计算的值进行打印
    std::cout << "  fp_coord_t (W=32, I=18) max_value: " << max_coord_val << std::endl;
    std::cout << "  fp_coord_t (W=32, I=18) min_value: " << min_coord_val << std::endl;
    std::cout << "  fp_vec_unit_t (W=24, I=3) max_value: " << max_vec_unit_val << std::endl;
    std::cout << "  fp_vec_unit_t (W=24, I=3) min_value: " << min_vec_unit_val << std::endl;
    std::cout << "-------------------------------------------\n" << std::endl;

    // --- 1. Basic Arithmetic Operations Test ---
    std::cout << "\n--- 1. Basic Arithmetic Operations Test ---" << std::endl;

    // fp_coord_t 类型测试 (W=32, I=18, F=14): 范围 [-131072.0, ~131072.0)。AP_SAT 模式。
    fp::fp_coord_t c_pos_a = 10.125;
    fp::fp_coord_t c_pos_b = 5.5;
    fp::fp_coord_t c_neg_a = -10.125;
    fp::fp_coord_t c_neg_b = -5.5;
    fp::fp_coord_t c_zero = 0.0;
    fp::fp_coord_t c_one = 1.0;
    fp::fp_coord_t c_large_val_in = 200000.0; // 超出 I=18 范围

    std::cout << "fp_coord_t (W=32, I=18, F=14):" << std::endl;
    std::cout << "  " << c_pos_a << " + " << c_pos_b << " = " << fp::add(c_pos_a, c_pos_b) << " (Expected: 15.625)" << std::endl;
    std::cout << "  " << c_pos_a << " - " << c_pos_b << " = " << fp::sub(c_pos_a, c_pos_b) << " (Expected: 4.625)" << std::endl;
    std::cout << "  " << c_pos_a << " * " << c_pos_b << " = " << fp::mul(c_pos_a, c_pos_b) << " (Expected: 55.6875)" << std::endl;
    std::cout << "  " << c_pos_a << " / " << c_pos_b << " = " << fp::div(c_pos_a, c_pos_b) << " (Expected: 1.840881348 - actual fp_coord_t result)" << std::endl; 
    std::cout << "  " << c_neg_a << " + " << c_neg_b << " = " << fp::add(c_neg_a, c_neg_b) << " (Expected: -15.625)" << std::endl;
    std::cout << "  " << c_neg_a << " * " << c_neg_b << " = " << fp::mul(c_neg_a, c_neg_b) << " (Expected: 55.6875)" << std::endl;
    std::cout << "  " << c_zero << " + " << c_pos_b << " = " << fp::add(c_zero, c_pos_b) << " (Expected: 5.5)" << std::endl;
    std::cout << "  " << c_pos_a << " * " << c_zero << " = " << fp::mul(c_pos_a, c_zero) << " (Expected: 0)" << std::endl;
    std::cout << "  " << c_pos_a << " / " << c_zero << " = " << fp::div(c_pos_a, c_zero) << " (Expected: 0, due to division by zero handling)" << std::endl; 
    
    // 饱和行为测试 (AP_SAT 模式) - 针对I=18的饱和测试
    fp::fp_coord_t c_val_within_range = 15000.0; 
    std::cout << "  " << c_val_within_range << " + " << c_one << " = " << fp::add(c_val_within_range, c_one) << " (Expected: 15001.0, within I=18 range)" << std::endl; 

    // 200000.0 + 1.0 远超最大值，应饱和到 max_coord_val
    fp::fp_coord_t saturated_pos_coord = fp::add(c_large_val_in, c_one);
    std::cout << "  " << c_large_val_in << " + " << c_one << " = " << saturated_pos_coord << " (Expected: " << max_coord_val << ", due to AP_SAT for I=18, Match: " << std::boolalpha << is_close(saturated_pos_coord, max_coord_val) << ")" << std::endl; 
    // -200000.0 - 1.0 远超最小值，应饱和到 min_coord_val
    fp::fp_coord_t saturated_neg_coord = fp::sub((fp::fp_coord_t)(-c_large_val_in), c_one);
    std::cout << "  " << (fp::fp_coord_t)(-c_large_val_in) << " - " << c_one << " = " << saturated_neg_coord << " (Expected: " << min_coord_val << ", due to AP_SAT for I=18, Match: " << std::boolalpha << is_close(saturated_neg_coord, min_coord_val) << ")" << std::endl; 

    std::cout << "  " << c_zero << " + " << c_one << " = " << fp::add(c_zero, c_one) << " (Expected: 1)" << std::endl;
    std::cout << "  " << c_zero << " - " << c_one << " = " << fp::sub(c_zero, c_one) << " (Expected: -1)" << std::endl;


    // fp_vec_unit_t 类型测试 (W=24, I=3, F=21): 范围 [-4.0, ~4.0)。AP_SAT 模式。
    fp::fp_vec_unit_t v_pos_a = 0.75;
    fp::fp_vec_unit_t v_pos_b = 0.25;
    fp::fp_vec_unit_t v_neg_a = -0.75;
    fp::fp_vec_unit_t v_zero = 0.0;
    fp::fp_vec_unit_t v_one = 1.0;
    // 定义一个会触发饱和的值 (1 / 0.25 = 4.0, 刚好在 I=3 的最大值)
    fp::fp_vec_unit_t v_saturate_val = 0.25; 
    // 定义一个超出 I=3 范围的值 (如 5.0)
    fp::fp_vec_unit_t v_large_val_in = 5.0; 

    std::cout << "\nfp_vec_unit_t (W=24, I=3, F=21):" << std::endl;
    std::cout << "  " << v_pos_a << " * " << v_pos_b << " = " << fp::mul(v_pos_a, v_pos_b) << " (Expected: 0.1875)" << std::endl;
    std::cout << "  " << v_neg_a << " * " << v_pos_b << " = " << fp::mul(v_neg_a, v_pos_b) << " (Expected: -0.1875)" << std::endl;
    
    // 测试除法饱和行为：1 / 0.25 = 4.0，刚好饱和到 max_vec_unit_val (由于 TRN 模式，4.0 会被截断为 3.999...)
    fp::fp_vec_unit_t saturated_div_vec_unit = fp::div(v_one, v_saturate_val);
    std::cout << "  " << v_one << " / " << v_saturate_val << " = " << saturated_div_vec_unit << " (Expected: " << max_vec_unit_val << ", due to AP_SAT for I=3, Match: " << std::boolalpha << is_close(saturated_div_vec_unit, max_vec_unit_val) << ")" << std::endl; 
    
    // 测试除零处理
    std::cout << "  " << v_one << " / " << v_zero << " = " << fp::div(v_one, v_zero) << " (Expected: 0, due to division by zero handling)" << std::endl; 
    
    // 5.0 超出 I=3 范围 [-4.0, ~4.0)，AP_SAT 会将其饱和到 max_vec_unit_val
    fp::fp_vec_unit_t saturated_mul_vec_unit = fp::mul(v_large_val_in, v_one);
    std::cout << "  " << v_large_val_in << " * " << v_one << " = " << saturated_mul_vec_unit << " (Expected: " << max_vec_unit_val << ", due to AP_SAT for I=3, Match: " << std::boolalpha << is_close(saturated_mul_vec_unit, max_vec_unit_val) << ")" << std::endl; 

    // --- 2. Complex Operations Test (Iterative sqrt/inv_sqrt, NUM_ITERATIONS=5) ---
    std::cout << "\n--- 2. Complex Operations Test (Iterative sqrt/inv_sqrt, NUM_ITERATIONS=5) ---" << std::endl;

    // sqrt/inv_sqrt (fp_coord_t)
    fp::fp_coord_t c_sqrt_in_16 = 16.0;
    fp::fp_coord_t c_sqrt_in_0 = 0.0;
    fp::fp_coord_t c_sqrt_in_neg = -4.0;
    fp::fp_coord_t c_sqrt_in_1 = 1.0;
    fp::fp_coord_t c_sqrt_in_small = 0.0009765625; // 刚好是 2^-10
    fp::fp_coord_t c_sqrt_in_large = 25000.0; 
    fp::fp_coord_t c_sqrt_in_frac = 0.25; 

    std::cout << "Square Root/Inverse Square Root (fp_coord_t):" << std::endl;
    std::cout << "  sqrt(" << c_sqrt_in_16 << ") = " << fp::sqrt(c_sqrt_in_16) << " (Expected: ~4.0)" << std::endl;
    std::cout << "  inv_sqrt(" << c_sqrt_in_16 << ") = " << fp::inv_sqrt(c_sqrt_in_16) << " (Expected: ~0.25)" << std::endl;
    std::cout << "  sqrt(" << c_sqrt_in_0 << ") = " << fp::sqrt(c_sqrt_in_0) << " (Expected: 0.0)" << std::endl;
    std::cout << "  inv_sqrt(" << c_sqrt_in_0 << ") = " << fp::inv_sqrt(c_sqrt_in_0) << " (Expected: 0.0)" << std::endl;
    std::cout << "  sqrt(" << c_sqrt_in_neg << ") = " << fp::sqrt(c_sqrt_in_neg) << " (Expected: 0.0) (Negative input handled)" << std::endl;
    std::cout << "  sqrt(" << c_sqrt_in_1 << ") = " << fp::sqrt(c_sqrt_in_1) << " (Expected: ~1.0)" << std::endl;
    std::cout << "  sqrt(" << c_sqrt_in_small << ") = " << fp::sqrt(c_sqrt_in_small) << " (Expected: ~0.03125)" << std::endl; 
    std::cout << "  sqrt(" << c_sqrt_in_large << ") = " << fp::sqrt(c_sqrt_in_large) << " (Expected: ~158.113883, actual fp_coord_t result)" << std::endl; 
    std::cout << "  inv_sqrt(" << c_sqrt_in_large << ") = " << fp::inv_sqrt(c_sqrt_in_large) << " (Expected: ~0.0063245, actual fp_coord_t result)" << std::endl; 
    std::cout << "  sqrt(" << c_sqrt_in_frac << ") = " << fp::sqrt(c_sqrt_in_frac) << " (Expected: ~0.5)" << std::endl;
    
    // sqrt (fp_vec_unit_t)
    fp::fp_vec_unit_t v_sqrt_in_0_09 = 0.09;
    fp::fp_vec_unit_t v_sqrt_in_0 = 0.0;
    fp::fp_vec_unit_t v_sqrt_in_neg = -0.1;
    fp::fp_vec_unit_t v_sqrt_in_1 = 1.0;
    fp::fp_vec_unit_t v_sqrt_in_large_comp = 1.5; 
    fp::fp_vec_unit_t v_sqrt_in_frac = 0.81; 

    std::cout << "\nSquare Root (fp_vec_unit_t):" << std::endl;
    std::cout << "  sqrt(" << v_sqrt_in_0_09 << ") = " << fp::sqrt(v_sqrt_in_0_09) << " (Expected: ~0.30)" << std::endl; // sqrt(0.09)=0.3
    // 1/sqrt(0.09) = 3.33333333...
    // 此值在 fp_vec_unit_t (I=3, 范围 [-4.0, ~4.0)) 的范围内，不会饱和。
    fp::fp_vec_unit_t inv_sqrt_v_0_09 = fp::inv_sqrt(v_sqrt_in_0_09);
    std::cout << "  inv_sqrt(" << inv_sqrt_v_0_09 << ") = " << inv_sqrt_v_0_09 << " (Expected: ~3.333333333, Match: " << std::boolalpha << is_close(inv_sqrt_v_0_09, (fp::fp_vec_unit_t)3.333333333) << ")" << std::endl; 
    std::cout << "  sqrt(" << v_sqrt_in_0 << ") = " << fp::sqrt(v_sqrt_in_0) << " (Expected: 0.0)" << std::endl;
    std::cout << "  inv_sqrt(" << v_sqrt_in_0 << ") = " << fp::inv_sqrt(v_sqrt_in_0) << " (Expected: 0.0)" << std::endl;
    std::cout << "  sqrt(" << v_sqrt_in_neg << ") = " << fp::sqrt(v_sqrt_in_neg) << " (Expected: 0.0) (Negative input handled)" << std::endl;
    std::cout << "  sqrt(1) = " << fp::sqrt(v_sqrt_in_1) << " (Expected: ~1.0)" << std::endl;
    std::cout << "  sqrt(" << v_sqrt_in_large_comp << ") = " << fp::sqrt(v_sqrt_in_large_comp) << " (Expected: ~1.22474487)" << std::endl; // sqrt(1.5)
    std::cout << "  inv_sqrt(" << v_sqrt_in_large_comp << ") = " << fp::inv_sqrt(v_sqrt_in_large_comp) << " (Expected: ~0.81649658)" << std::endl; // 1/sqrt(1.5)
    std::cout << "  sqrt(" << v_sqrt_in_frac << ") = " << fp::sqrt(v_sqrt_in_frac) << " (Expected: ~0.9)" << std::endl;

    // clamp/saturate
    std::cout << "\nClamp/Saturate Test:" << std::endl;
    fp::fp_coord_t c_clamp_val = 100.0;
    fp::fp_coord_t c_clamp_min_lim = 0.0; 
    fp::fp_coord_t c_clamp_max_lim = 50.0;
    std::cout << "  clamp(" << c_clamp_val << ", " << c_clamp_min_lim << ", " << c_clamp_max_lim << ") = " << fp::clamp(c_clamp_val, c_clamp_min_lim, c_clamp_max_lim) << " (Expected: 50.0)" << std::endl;
    std::cout << "  clamp(" << c_neg_a << ", " << c_clamp_min_lim << ", " << c_clamp_max_lim << ") = " << fp::clamp(c_neg_a, c_clamp_min_lim, c_clamp_max_lim) << " (Expected: 0.0)" << std::endl;
    std::cout << "  saturate(" << c_clamp_val << ") = " << fp::saturate(c_clamp_val) << " (Expected: 1.0)" << std::endl;
    std::cout << "  saturate(" << c_neg_a << ") = " << fp::saturate(c_neg_a) << " (Expected: 0.0)" << std::endl;
    std::cout << "  saturate(" << v_pos_a << ") = " << fp::saturate(v_pos_a) << " (Expected: 0.75)" << std::endl;


    // --- 3. Linear Algebra Operations Test ---
    std::cout << "\n--- 3. Linear Algebra Operations Test ---" << std::endl;

    // 向量运算 (fp_vec_unit_t)
    // 向量分量值现在都必须在 fp_vec_unit_t 的范围 [-4.0, ~4.0) 内
    fp::vec3_fp_vec_unit vecA = {0.70710678, 0.70710678, 0.0}; 
    fp::vec3_fp_vec_unit vecB = {0.0, 0.70710678, 0.70710678};
    fp::vec3_fp_vec_unit vecC = {0.5, 1.0, 1.5}; 
    fp::vec3_fp_vec_unit vec_zero_unit = {0.0, 0.0, 0.0};
    fp::vec3_fp_vec_unit vec_neg_A = {-0.70710678, -0.70710678, 0.0};
    fp::fp_vec_unit_t vec_large_component_val_scalar = 1.5;
    fp::vec3_fp_vec_unit vec_large_component = {vec_large_component_val_scalar, vec_large_component_val_scalar, vec_large_component_val_scalar}; 

    std::cout << "Vector Operations (fp_vec_unit_t):" << std::endl;
    std::cout << "  vecA: (" << vecA.x << ", " << vecA.y << ", " << vecA.z << ")" << std::endl;
    std::cout << "  vecB: (" << vecB.x << ", " << vecB.y << ", " << vecB.z << ")" << std::endl;
    std::cout << "  vecC (non-unit): (" << vecC.x << ", " << vecC.y << ", " << vecC.z << ")" << std::endl; 

    fp::vec3_fp_vec_unit vec_add_res = fp::vec3_add(vecA, vecB);
    std::cout << "  vec_add (A+B): (" << vec_add_res.x << ", " << vec_add_res.y << ", " << vec_add_res.z << ") (Expected: ~0.70710678, ~1.41421356, ~0.70710678)" << std::endl;
    fp::vec3_fp_vec_unit vec_sub_res = fp::vec3_sub(vecA, vecB);
    std::cout << "  vec_sub (A-B): (" << vec_sub_res.x << ", " << vec_sub_res.y << ", " << vec_sub_res.z << ") (Expected: ~0.70710678, 0.0, ~-0.70710678)" << std::endl;
    fp::fp_vec_unit_t dot_prod = fp::vec3_dot(vecA, vecB);
    std::cout << "  dot_prod (A.B): " << dot_prod << " (Expected: ~0.5)" << std::endl;
    fp::fp_vec_unit_t dot_prod_self = fp::vec3_dot(vecA, vecA);
    std::cout << "  dot_prod (A.A): " << dot_prod_self << " (Expected: ~1.0)" << std::endl;
    fp::fp_vec_unit_t dot_prod_ortho = fp::vec3_dot(vecA, {0.70710678, -0.70710678, 0.0});
    std::cout << "  dot_prod (A.Orthogonal): " << dot_prod_ortho << " (Expected: ~0.0)" << std::endl;
    fp::fp_vec_unit_t dot_prod_neg = fp::vec3_dot(vecA, vec_neg_A);
    std::cout << "  dot_prod (A.-A): " << dot_prod_neg << " (Expected: ~-1.0)" << std::endl;


    fp::vec3_fp_vec_unit cross_prod = fp::vec3_cross(vecA, vecB);
    std::cout << "  cross_prod (A x B): (" << cross_prod.x << ", " << cross_prod.y << ", " << cross_prod.z << ") (Expected: ~0.5, ~-0.5, ~0.5)" << std::endl;
    fp::fp_vec_unit_t len_sq_A = fp::vec3_length_sq(vecA);
    std::cout << "  len_sq_A: " << len_sq_A << " (Expected: ~1.0)" << std::endl;
    fp::fp_vec_unit_t len_A = fp::vec3_length(vecA);
    std::cout << "  len_A: " << len_A << " (Expected: ~1.0)" << std::endl;
    fp::vec3_fp_vec_unit norm_A = fp::vec3_normalize(vecA);
    std::cout << "  norm_A: (" << norm_A.x << ", " << norm_A.y << ", " << norm_A.z << ") (Expected: ~0.70710678, ~0.70710678, 0.0)" << std::endl;
    
    // vecC = {0.5, 1.0, 1.5}， length_sq = 0.25 + 1.0 + 2.25 = 3.5
    // 3.5 在 I=3 的范围 [-4.0, ~4.0) 内，不会饱和。
    fp::fp_vec_unit_t len_sq_C = fp::vec3_length_sq(vecC);
    std::cout << "  len_sq_C (from (0.5,1.0,1.5), expected 3.5): " << len_sq_C << " (Expected: 3.5, Match: " << std::boolalpha << is_close(len_sq_C, (fp::fp_vec_unit_t)3.5) << ")" << std::endl;
    // 归一化结果将基于未饱和的 len_sq_C (3.5) 进行计算
    // inv_sqrt(3.5) = 1 / 1.87082869 = 0.53452248
    // norm_C.x = 0.5 * 0.53452248 = 0.26726124
    // norm_C.y = 1.0 * 0.53452248 = 0.53452248
    // norm_C.z = 1.5 * 0.53452248 = 0.80178372
    fp::vec3_fp_vec_unit norm_C = fp::vec3_normalize(vecC); 
    std::cout << "  norm_C (from (0.5,1.0,1.5)): (" << norm_C.x << ", " << norm_C.y << ", " << norm_C.z << ") (Expected: ~0.26726124, ~0.53452248, ~0.80178372 based on actual length_sq)" << std::endl; 
    
    fp::vec3_fp_vec_unit norm_zero = fp::vec3_normalize(vec_zero_unit); 
    std::cout << "  norm_zero: (" << norm_zero.x << ", " << norm_zero.y << ", " << norm_zero.z << ") (Expected: 0.0, 0.0, 0.0)" << std::endl;
    
    // vec_large_component = {1.5, 1.5, 1.5}， length_sq = 2.25 + 2.25 + 2.25 = 6.75
    // 6.75 超过 I=3 的范围 [-4.0, ~4.0)，会饱和到 max_vec_unit_val (~3.999999523)
    fp::fp_vec_unit_t len_sq_large_comp = fp::vec3_length_sq(vec_large_component);
    std::cout << "  len_sq_large_comp (from (1.5,1.5,1.5), expected 6.75): " << len_sq_large_comp << " (Actual: " << len_sq_large_comp << ", due to AP_SAT for I=3, Match: " << std::boolalpha << is_close(len_sq_large_comp, max_vec_unit_val) << ")" << std::endl;
    // 归一化结果将基于饱和后的 len_sq_large_comp (max_vec_unit_val) 进行计算
    // inv_len = 1 / sqrt(max_vec_unit_val) = 1 / sqrt(3.999999523) = 1 / 1.99999988 = 0.50000003
    // norm_large_comp.x = 1.5 * 0.50000003 = 0.750000045
    // norm_large_comp.y = 1.5 * 0.50000003 = 0.750000045
    // norm_large_comp.z = 1.5 * 0.50000003 = 0.750000045
    fp::vec3_fp_vec_unit norm_large_comp = fp::vec3_normalize(vec_large_component); 
    std::cout << "  norm_large_comp (from (1.5,1.5,1.5)): (" << norm_large_comp.x << ", " << norm_large_comp.y << ", " << norm_large_comp.z << ") (Expected: ~0.750000045, ~0.750000045, ~0.750000045 based on saturated length_sq)" << std::endl; 

    // 向量运算 (fp_coord_t)
    fp::vec3_fp_coord c_vec_large = {100.0, 200.0, 300.0};
    fp::vec3_fp_coord c_vec_large_neg = {-100.0, -200.0, -300.0};
    fp::vec3_fp_coord c_vec_zero = {0.0, 0.0, 0.0};
    
    // 100^2 + 200^2 + 300^2 = 10000 + 40000 + 90000 = 140000.0
    // 140000.0 超出 fp_coord_t (I=18, 范围 [-131072.0, ~131072.0))
    // 会饱和到 max_coord_val (~131071.9999389)
    fp::fp_coord_t c_len_sq_large = fp::vec3_length_sq(c_vec_large);
    std::cout << "\nVector Operations (fp_coord_t):" << std::endl;
    std::cout << "  vec_large: (" << c_vec_large.x << ", " << c_vec_large.y << ", " << c_vec_large.z << ")" << std::endl;
    std::cout << "  len_sq_large (expected 140000.0): " << c_len_sq_large << " (Actual: " << max_coord_val << ", due to AP_SAT for I=18, Match: " << std::boolalpha << is_close(c_len_sq_large, max_coord_val) << ")" << std::endl; 
    
    // sqrt(max_coord_val) = sqrt(131071.9999389) = ~362.03867
    fp::fp_coord_t c_len_large = fp::vec3_length(c_vec_large);
    std::cout << "  len_large: " << c_len_large << " (Expected: ~362.03867 from saturated value, actual fp_coord_t result)" << std::endl; 
    
    fp::vec3_fp_coord c_norm_large = fp::vec3_normalize(c_vec_large); 
    // inv_len = 1 / ~362.03867 = ~0.0027621
    // norm_large.x = 100 * ~0.0027621 = ~0.27621
    // norm_large.y = 200 * ~0.0027621 = ~0.55242
    // norm_large.z = 300 * ~0.0027621 = ~0.82863
    std::cout << "  norm_large: (" << c_norm_large.x << ", " << c_norm_large.y << ", " << c_norm_large.z << ") (Expected: ~0.27621, ~0.55242, ~0.82863 based on saturated length_sq, actual fp_coord_t result)" << std::endl; 
    
    fp::vec3_fp_coord c_norm_large_neg = fp::vec3_normalize(c_vec_large_neg);
    std::cout << "  norm_large_neg: (" << c_norm_large_neg.x << ", " << c_norm_large_neg.y << ", " << c_norm_large_neg.z << ") (Expected: ~-0.27621, ~-0.55242, ~-0.82863 based on saturated length_sq, actual fp_coord_t result)" << std::endl; 
    fp::vec3_fp_coord c_norm_zero = fp::vec3_normalize(c_vec_zero);
    std::cout << "  norm_zero: (" << c_norm_zero.x << ", " << c_norm_zero.y << ", " << c_norm_zero.z << ") (Expected: 0.0, 0.0, 0.0)" << std::endl;


    // 矩阵-向量乘法 (4x4 矩阵 * 4元素向量 - fp_coord_t)
    std::cout << "\nMatrix-Vector Multiplication (fp_coord_t):" << std::endl;
    
    fp::fp_coord_t identity_matrix[4][4] = {
        {1.0, 0.0, 0.0, 0.0},
        {0.0, 1.0, 0.0, 0.0},
        {0.0, 0.0, 1.0, 0.0},
        {0.0, 0.0, 0.0, 1.0}
    };
    fp::vec3_fp_coord vec_in_coord = {10.0, 20.0, 30.0};
    fp::fp_coord_t w_val = 1.0;
    fp::vec3_fp_coord mat_vec_res_id = fp::mat4_vec4_mul(identity_matrix, vec_in_coord, w_val);
    std::cout << "  Input vector: (" << vec_in_coord.x << ", " << vec_in_coord.y << ", " << vec_in_coord.z << ")" << std::endl;
    std::cout << "  Identity Matrix Result: (" << mat_vec_res_id.x << ", " << mat_vec_res_id.y << ", " << mat_vec_res_id.z << ") (Expected: 10, 20, 30)" << std::endl;

    fp::fp_coord_t translate_matrix[4][4] = {
        {1.0, 0.0, 0.0, 1.0},
        {0.0, 1.0, 0.0, 2.0},
        {0.0, 0.0, 1.0, 3.0},
        {0.0, 0.0, 0.0, 1.0}
    };
    fp::vec3_fp_coord mat_vec_res_trans = fp::mat4_vec4_mul(translate_matrix, vec_in_coord, w_val);
    std::cout << "  Translation Matrix Result: (" << mat_vec_res_trans.x << ", " << mat_vec_res_trans.y << ", " << mat_vec_res_trans.z << ") (Expected: 11, 22, 33)" << std::endl;

    fp::fp_coord_t scale_matrix[4][4] = {
        {2.0, 0.0, 0.0, 0.0},
        {0.0, 2.0, 0.0, 0.0},
        {0.0, 0.0, 2.0, 0.0},
        {0.0, 0.0, 0.0, 1.0}
    };
    fp::vec3_fp_coord mat_vec_res_scale = fp::mat4_vec4_mul(scale_matrix, vec_in_coord, w_val);
    std::cout << "  Scaling Matrix Result: (" << mat_vec_res_scale.x << ", " << mat_vec_res_scale.y << ", " << mat_vec_res_scale.z << ") (Expected: 20, 40, 60)" << std::endl;

    fp::fp_coord_t neg_scale_matrix[4][4] = {
        {-1.0, 0.0, 0.0, 0.0},
        {0.0, -1.0, 0.0, 0.0},
        {0.0, 0.0, -1.0, 0.0},
        {0.0, 0.0, 0.0, 1.0}
    };
    fp::vec3_fp_coord mat_vec_res_neg_scale = fp::mat4_vec4_mul(neg_scale_matrix, vec_in_coord, w_val);
    std::cout << "  Negative Scaling Matrix Result: (" << mat_vec_res_neg_scale.x << ", " << mat_vec_res_neg_scale.y << ", " << mat_vec_res_neg_scale.z << ") (Expected: -10, -20, -30)" << std::endl;


    // 矩阵-向量乘法 (3x3 矩阵 * 3元素向量 - fp_vec_unit_t)
    std::cout << "\nMatrix-Vector Multiplication (fp_vec_unit_t):" << std::endl;
    fp::fp_vec_unit_t rotation_matrix_xy_90[3][3] = { 
        {0.0, -1.0, 0.0},
        {1.0, 0.0, 0.0},
        {0.0, 0.0, 1.0}
    };
    fp::vec3_fp_vec_unit vec_in_unit = {0.5, 0.5, 0.0};
    fp::vec3_fp_vec_unit mat3_vec3_res = fp::mat3_vec3_mul(rotation_matrix_xy_90, vec_in_unit);
    std::cout << "  Input unit vector: (" << vec_in_unit.x << ", " << vec_in_unit.y << ", " << vec_in_unit.z << ")" << std::endl;
    std::cout << "  Rotation Matrix Result: (" << mat3_vec3_res.x << ", " << mat3_vec3_res.y << ", " << mat3_vec3_res.z << ") (Expected: -0.5, 0.5, 0.0)" << std::endl;

    fp::fp_vec_unit_t general_matrix[3][3] = { 
        {0.1, 0.2, 0.3},
        {0.4, 0.5, 0.6},
        {0.7, 0.8, 0.9}
    };
    fp::vec3_fp_vec_unit vec_general_in = {0.1, 0.2, 0.3};
    fp::vec3_fp_vec_unit mat3_vec3_general_res = fp::mat3_vec3_mul(general_matrix, vec_general_in);
    std::cout << "  Input general vector: (" << vec_general_in.x << ", " << vec_general_in.y << ", " << vec_general_in.z << ")" << std::endl;
    std::cout << "  General Matrix Result: (" << mat3_vec3_general_res.x << ", " << mat3_vec3_general_res.y << ", " << mat3_vec3_general_res.z << ") (Expected: ~0.14, ~0.32, ~0.50)" << std::endl;


    // --- 4. Fixed-point Type Conversion Test ---
    std::cout << "\n--- 4. Fixed-point Type Conversion Test ---" << std::endl;

    fp::fp_coord_t c_val_for_conv = 0.75;
    fp::fp_vec_unit_t v_res_conv = fp::conv_coord_to_vec_unit(c_val_for_conv);
    std::cout << "  conv_coord_to_vec_unit(" << c_val_for_conv << ") = " << v_res_conv << " (Expected: 0.75)" << std::endl;

    fp::fp_coord_t c_val_large_for_conv = 1000.0;
    // 1000.0 (fp_coord_t) -> fp_vec_unit_t (I=3, range [-4, ~4))
    // 1000.0 会饱和到 max_vec_unit_val
    fp::fp_vec_unit_t v_res_conv_large = fp::conv_coord_to_vec_unit(c_val_large_for_conv);
    std::cout << "  conv_coord_to_vec_unit(" << c_val_large_for_conv << ") = " << v_res_conv_large << " (Expected: " << max_vec_unit_val << ", due to AP_SAT for I=3, Match: " << std::boolalpha << is_close(v_res_conv_large, max_vec_unit_val) << ")" << std::endl; 

    fp::fp_vec_unit_t v_val_for_conv = 0.8;
    fp::fp_coord_t c_res_conv = fp::conv_vec_unit_to_coord(v_val_for_conv);
    std::cout << "  conv_vec_unit_to_coord(" << v_val_for_conv << ") = " << c_res_conv << " (Expected: ~0.799987793)" << std::endl; // F=22 精度降低到 F=14

    // Color Conversion (fp_vec_unit_t to fp_color_channel_t)
    std::cout << "\nColor Conversion (fp_vec_unit_t to fp_color_channel_t):" << std::endl;
    fp::fp_vec_unit_t color_val_norm_0 = 0.0;
    fp::fp_vec_unit_t color_val_norm_half = 0.5; 
    fp::fp_vec_unit_t color_val_norm_one = 1.0;
    fp::fp_vec_unit_t color_val_norm_neg = -0.1; 
    fp::fp_vec_unit_t color_val_norm_over = 1.5; 
    fp::fp_vec_unit_t color_val_norm_mid_round = 0.501; // 0.501 * 255 = 127.755 -> 128
    fp::fp_vec_unit_t color_val_norm_low_mid_round = 0.4989998341; // 0.499 * 255 = 127.245 -> 127
    fp::fp_vec_unit_t color_val_norm_high_frac = 0.999; // 0.999 * 255 = 254.745 -> 255
    fp::fp_vec_unit_t color_val_norm_low_frac = 0.001; // 0.001 * 255 = 0.255 -> 0

    std::cout << "  Normalized value: " << color_val_norm_0 << " -> Color channel: " << (unsigned int)fp::conv_fp_to_color_channel(color_val_norm_0) << " (Expected: 0)" << std::endl;
    std::cout << "  Normalized value: " << color_val_norm_half << " -> Color channel: " << (unsigned int)fp::conv_fp_to_color_channel(color_val_norm_half) << " (Expected: 128)" << std::endl; 
    std::cout << "  Normalized value: " << color_val_norm_one << " -> Color channel: " << (unsigned int)fp::conv_fp_to_color_channel(color_val_norm_one) << " (Expected: 255)" << std::endl;
    std::cout << "  Normalized value: " << color_val_norm_neg << " -> Color channel: " << (unsigned int)fp::conv_fp_to_color_channel(color_val_norm_neg) << " (Expected: 0, due to saturate)" << std::endl;
    std::cout << "  Normalized value: " << color_val_norm_over << " -> Color channel: " << (unsigned int)fp::conv_fp_to_color_channel(color_val_norm_over) << " (Expected: 255, due to saturate)" << std::endl;
    std::cout << "  Normalized value: " << color_val_norm_mid_round << " -> Color channel: " << (unsigned int)fp::conv_fp_to_color_channel(color_val_norm_mid_round) << " (Expected: 128)" << std::endl; 
    std::cout << "  Normalized value: " << color_val_norm_low_mid_round << " -> Color channel: " << (unsigned int)fp::conv_fp_to_color_channel(color_val_norm_low_mid_round) << " (Expected: 127)" << std::endl; 
    std::cout << "  Normalized value: " << color_val_norm_high_frac << " -> Color channel: " << (unsigned int)fp::conv_fp_to_color_channel(color_val_norm_high_frac) << " (Expected: 255)" << std::endl; 
    std::cout << "  Normalized value: " << color_val_norm_low_frac << " -> Color channel: " << (unsigned int)fp::conv_fp_to_color_channel(color_val_norm_low_frac) << " (Expected: 0)" << std::endl; 

    // Color Conversion (fp_coord_t to fp_color_channel_t)
    std::cout << "\nColor Conversion (fp_coord_t to fp_color_channel_t):" << std::endl;
    fp::fp_coord_t c_color_val_norm_half = 0.5;
    fp::fp_coord_t c_color_val_over = 10.0; 
    std::cout << "  Normalized value: " << c_color_val_norm_half << " -> Color channel: " << (unsigned int)fp::conv_fp_to_color_channel(c_color_val_norm_half) << " (Expected: 128)" << std::endl; 
    std::cout << "  Normalized value: " << c_color_val_over << " -> Color channel: " << (unsigned int)fp::conv_fp_to_color_channel(c_color_val_over) << " (Expected: 255, due to saturate)" << std::endl;


    // --- 5. HLS Wrapper Function Test (fp_coord_t, fp_vec_unit_t, fp_color_channel_t)
    std::cout << "\n--- 5. HLS Wrapper Function Test (fp_coord_t, fp_vec_unit_t, fp_color_channel_t) ---" << std::endl;
    
    fp::fp_coord_t wrap_in_x = 3.0;
    fp::fp_coord_t wrap_in_y = 4.0;
    fp::fp_coord_t wrap_in_z = 0.0;
    fp::fp_vec_unit_t wrap_in_u = 0.6;
    fp::fp_vec_unit_t wrap_in_v = 0.8;
    
    fp::fp_color_channel_t wrap_out_color_r;
    fp::fp_coord_t wrap_out_coord_len_sq;
    fp::fp_vec_unit_t wrap_out_vec_norm_x;

    std::cout << "  Input coord: (" << wrap_in_x << ", " << wrap_in_y << ", " << wrap_in_z << ")" << std::endl;
    std::cout << "  Input vec unit: (" << wrap_in_u << ", " << wrap_in_v << ", 0.5)" << std::endl;

    fpal_wrapper(wrap_in_x, wrap_in_y, wrap_in_z, wrap_in_u, wrap_in_v,
                 wrap_out_color_r, wrap_out_coord_len_sq, wrap_out_vec_norm_x);

    // coord_len_sq: 3^2 + 4^2 + 0^2 = 9 + 16 + 0 = 25.0。 在 I=18 范围内，不会饱和。
    std::cout << "  Output coordinate length squared: " << wrap_out_coord_len_sq << " (Expected: 25.0)" << std::endl;
    
    // vec_unit = {0.6, 0.8, 0.5}
    // len_sq = 0.6^2 + 0.8^2 + 0.5^2 = 0.36 + 0.64 + 0.25 = 1.25
    // 1.25 在 fp_vec_unit_t (I=3) 范围内 [-4.0, ~4.0)。不会饱和。
    // inv_len = 1 / sqrt(1.25) = 1 / ~1.118034 = ~0.894427
    // normalized_vec.x = 0.6 * ~0.894427 = ~0.536656
    // normalized_vec.y = 0.8 * ~0.894427 = ~0.715541
    std::cout << "  Output normalized vector X component: " << wrap_out_vec_norm_x << " (Expected: ~0.536656)" << std::endl;
    
    // out_color_r = conv_fp_to_color_channel(normalized_vec.y)
    // normalized_vec.y = ~0.715541
    // ~0.715541 * 255 = ~182.462955 -> round -> 182
    std::cout << "  Output color R component (from normalized vector Y): " << (unsigned int)wrap_out_color_r << " (Expected: 182)" << std::endl; 

    fp::fp_coord_t wrap_in_x_zero = 0.0;
    fp::fp_coord_t wrap_in_y_zero = 0.0;
    fp::fp_coord_t wrap_in_z_zero = 0.0;
    fp::fp_vec_unit_t wrap_in_u_zero = 0.0;
    fp::fp_vec_unit_t wrap_in_v_zero = 0.0;

    fpal_wrapper(wrap_in_x_zero, wrap_in_y_zero, wrap_in_z_zero, wrap_in_u_zero, wrap_in_v_zero,
                 wrap_out_color_r, wrap_out_coord_len_sq, wrap_out_vec_norm_x);
    std::cout << "\n  --- Wrapper with Zero Inputs ---" << std::endl;
    std::cout << "  Output coordinate length squared (zero input): " << wrap_out_coord_len_sq << " (Expected: 0.0)" << std::endl;
    std::cout << "  Output normalized vector X component (zero input): " << wrap_out_vec_norm_x << " (Expected: 0.0)" << std::endl; // vec3_normalize 内部会返回零向量
    std::cout << "  Output color R component (zero input): " << (unsigned int)wrap_out_color_r << " (Expected: 0)" << std::endl;


    std::cout << "\n--- Testbench Completed ---" << std::endl;
    return 0;
}
